package GUI;

import Parser.*;
import DecisionMaker.*;
import java.awt.*;
import javax.swing.*;
import java.util.*;

/**
 * Class Steering, simulates the steering of a vehicle.
 */
public class Steering extends JPanel implements Process
{
  private Vector obstacleVector; // the vector which keeps the obstacles
  private final int WIDTH = 700, HEIGHT = 700; // the width and heigth of the scene

  private double rWidth; // the width of the robot
  private double rLength; // the length of the robot
  private double rX; // the x-coordinate of the robot
  private double rY; // the y-coordinate of the robot
  private double rTheta; // the theta-coordinate of the robot

  private double velocity = 20;
  private double steeringAngle = 0;

  private boolean traceOn = false,
      driving = false;

  private KBParser parser;
  private DecisionMaker DMMaker;

  private class Trace
  {
    public double X;
    public double Y;
    public double Theta;
    public Trace next;

    public Trace (double x, double y, double theta)
    {
      X = x;
      Y = y;
      Theta = theta;
      next = null;
    }
  }

  private Trace traceList = null;

  private void addTrace (Trace trace)
  {
    if (traceList == null)
    {
      traceList = trace;
    }
    else
    {
      Trace cursor;
      for (cursor = traceList; cursor.next != null; cursor = cursor.next);
      cursor.next = trace;
    }
  }

  /** Switches trace of the robot on and off.
   *
   * @param traceOn a boolean; if traceOn is true, the the trace is switched on.
   */
  public void setTrace (boolean traceOn)
  {
    this.traceOn = traceOn;
  }

  // class describing simple obstacles
  private class Obstacle
  {
    public double xmin;
    public double xmax;
    public double ymin;
    public double ymax;

    public Obstacle (double x1, double x2, double y1, double y2)
    {
      xmin = x1;
      xmax = x2;
      ymin = y1;
      ymax = y2;
    }
  }

  /** Creates the world. The trace is switched off.
   */
  public Steering (KBParser prsr)
  {
    parser = prsr;
    obstacleVector = new Vector();
    setSize(WIDTH, HEIGHT);

    addObstacle(-300,-40,-280,-240);
    addObstacle(40,300,-280,-240);
    addRobot(20, 30, 150, 220, -Math.PI / 2);

    setTrace(false);
  }

  // add an obstacle to the world
  private void addObstacle (double xmin, double xmax, double ymin, double ymax)
  {
    if (xmax < xmin || ymax < ymin)return;
    Obstacle newObst = new Obstacle(xmin, xmax, ymin, ymax);
    obstacleVector.addElement(newObst);
  }

  /**
   * Adds a robot to the world.
   *
   * @param width	a double, denoting the width of the simulated robot
   * @param length	a double, denoting the length of the simulated robot
   * @param x		a double, denoting the initial x coordinate of the robot
   * @param y		a double, denoting the initial y coordinate of the robot
   * @param theta	a double, denoting the initial angle of the robot relative to the x-axis; counter clockwise
   * has a positive sign
   */
  public void addRobot (double width, double length, double x, double y,
                        double theta)
  {
    rWidth = width;
    rLength = length;
    rX = x;
    rY = y;
    rTheta = theta;
    Trace newTrace = new Trace(rX, rY, rTheta);
    addTrace(newTrace);
  }

  // class needed to repaint the scene
  public void paintComponent (Graphics g)
  {
    super.paintComponent(g);
    setBackground(Color.white);
    g.setColor(getForeground());
    drawScene(g);
    moveCar();
  }

  // draw the scene
  private void drawScene (Graphics g)
  {
    drawObstacles(g);
    drawTrace(g);
  }

  private void drawTrace (Graphics g)
  {
    for (Trace cursor = traceList; cursor != null; cursor = cursor.next)
    {
      if (traceOn || (!traceOn && cursor.next == null))
      {
        rX = cursor.X;
        rY = cursor.Y;
        rTheta = cursor.Theta;
        drawRobot(g);
      }
    }
  }

  // draw the obstacles in the scene
  private void drawObstacles (Graphics g)
  {
    Enumeration enum1 = obstacleVector.elements();
    while (enum1.hasMoreElements())
    {
      Obstacle obst = (Obstacle) enum1.nextElement();
      drawSingleObstacle(g, obst);
    }
  }

  // draw a single obstale
  private void drawSingleObstacle (Graphics g, Obstacle obst)
  {
    drawStraightLine(g, obst.xmin, obst.ymin, obst.xmin, obst.ymax);
    drawStraightLine(g, obst.xmin, obst.ymax, obst.xmax, obst.ymax);
    drawStraightLine(g, obst.xmax, obst.ymax, obst.xmax, obst.ymin);
    drawStraightLine(g, obst.xmax, obst.ymin, obst.xmin, obst.ymin);
  }

  // draw the robot
  private void drawRobot (Graphics g)
  {
    double rAlpha = Math.atan(rWidth / rLength);
    double rHaDiag = Math.sqrt(Math.pow(rWidth, 2) + Math.pow(rLength, 2));

    double x1 = rX + rHaDiag * Math.cos(rTheta + rAlpha);
    double y1 = rY + rHaDiag * Math.sin(rTheta + rAlpha);

    double x2 = rX + rHaDiag * Math.cos(rTheta - rAlpha);
    double y2 = rY + rHaDiag * Math.sin(rTheta - rAlpha);

    double x3 = rX - rHaDiag * Math.cos(rTheta + rAlpha);
    double y3 = rY - rHaDiag * Math.sin(rTheta + rAlpha);

    double x4 = rX - rHaDiag * Math.cos(rTheta - rAlpha);
    double y4 = rY - rHaDiag * Math.sin(rTheta - rAlpha);

    drawStraightLine(g, x1, y1, x2, y2);
    drawStraightLine(g, x2, y2, x3, y3);
    drawStraightLine(g, x3, y3, x4, y4);
    drawStraightLine(g, x4, y4, x1, y1);
  }

  // draw a straight line
  private void drawStraightLine (Graphics g, double x1, double y1, double x2,
                                 double y2)
  {
    int xa = (int) x1 + WIDTH / 2;
    int xb = (int) x2 + WIDTH / 2;
    int ya = HEIGHT / 2 - (int) y1;
    int yb = HEIGHT / 2 - (int) y2;

    g.drawLine(xa, ya, xb, yb);
  }

  private double calculateDistance ()
  {
    return rY + 245;
  }

  private double calculateDeviation ()
  {
    return rX;
  }

  private double calculateInclination ()
  {
    return rTheta;
  }

  private void printVariable (String name, double value)
  {
    System.out.print(name);
    System.out.print(" = ");
    System.out.println(value);
  }

  /** The variables deviation and inclination can be asked for.
   * The deviation is the distance to the middle of the alley in which the vehicle should be steered.
   * The deviation is measured in pixels (between approximately -350 and 350).
   * The inclination (see module work book) is measured in radials.
   * Please note, that the inclination is the angle between the axis of the robot and the X-axis,
   * which means that if the inclination equals -PI/2, the robot is moving "straight downwards".
   */
  public double getVariable (String name)
  {
    if (name.equals("deviation"))return calculateDeviation();
    if (name.equals("inclination"))return calculateInclination();
    return 0;
  }

  /** The variable steeringAngle can be set.
   * The steering angle is measured in radials.
   */
  public void setVariable (String name, double value)
  {
    if (name.equals("steeringAngle")) steeringAngle = value;
    else
      System.out.println("ERROR: attempt to set unknown variable");
  }

  public void simulateStep (double deltaT)
  {
    DMMaker = new DecisionMaker(parser, this);
    DMMaker.ApplyRules();
    rX = rX + Math.cos(rTheta + steeringAngle) * velocity * deltaT;
    rY = rY + Math.sin(rTheta + steeringAngle) * velocity * deltaT;
    rTheta = rTheta + Math.sin(steeringAngle) / rLength * velocity * deltaT;
    Trace newTrace = new Trace(rX, rY, rTheta);
    addTrace(newTrace);
    repaint();
  }

  public void moveCar ()
  {
    if (driving)
    {
     simulateStep(0.05);
    }
  }

  public void setDriving (boolean action)
  {
    driving = action;
  }

  public boolean getDriving ()
  {
    return driving;
  }

  public String getProcessName()
  {
      return "Steering";
  }
}
